#pragma once

#include "g2o/core/base_multi_edge.h"
#include "lidar_localization/models/graph_optimizer/g2o/vertex/vertex_vec.hpp"

namespace g2o
{
  class EdgeGnss : public BaseMultiEdge<3, Eigen::Vector3d>
  {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    EdgeGnss();
    virtual bool read(std::istream &is) { return false; }
    virtual bool write(std::ostream &os) const { return false; }

    void computeError();

    // jacobian
    virtual void linearizeOplus();

    virtual void setMeasurement(const Eigen::Vector3d &m)
    {
      _measurement = m;
    }
    std::vector<g2o::MatrixX::MapType, Eigen::aligned_allocator<g2o::MatrixX::MapType>> GetJacobian() const
    {
      return _jacobianOplus;
    }
  };

} // namespace g2o